//
// Created by Sonny on 2023/10/4.
//

#ifndef DRV_DjiMotor_H
#define DRV_DjiMotor_H

#include "UPRE_Alg_PID.h"
#include "UPRE_Bsp_CAN.h"


enum Enum_Control_Method
{
    Control_Method_OPENLOOP = 0,
    Control_Method_TORQUE,
    Control_Method_OMEGA,
    Control_Method_ANGLE,
    Control_Method_OMEGA_ANGLE,
    Control_Method_TEST
};



class DjiMotor{
public:
    PID PID_Angle;
    PID PID_Omega;
    PID PID_Torque;
    PID PID_Test;

    void Init(Enum_Control_Method = Control_Method_TORQUE,float _Gearbox_Rate = 0.0f ,float _Torque_Max = 0.0f);
    void Get_Moter_Info(CAN_HandleTypeDef *hcan,uint8_t RxBuff[8]);
    void Output();
    void Control_PID();
    void Set_TargetParam(float Target_Angle,float Target_Omega);

    Enum_Control_Method Control_Method = Control_Method_ANGLE;
    const static uint32_t S2M_ID1 = 0X200,S2M_ID2 = 0X1FF;


    float Now_Angle = 0.0f;
    float Now_Omega = 0.0f;
    float Now_Torque = 0.0f;
    uint8_t Now_Temperature = 0;

    float Target_Angle = 0.0f;
    float Target_Omega = 0.0f;
    float Target_Torque = 0.0f;

    int OUT = 0.0f;

    uint32_t Torque_Max = 10000;//设定最大扭矩最大值
    int32_t Torque_Output = 10000;//电流最大输出值,-10000~10000
    float Gearbox_Rate = 36.0f;//减速比
    uint16_t Rx_Encoder = 0;//接收编码器位置,0~8192
    int16_t Rx_Omega = 0;//接受的速度,RPM
    int16_t Rx_Torque = 0;//接受的的扭矩.-30000~30000
    uint16_t Rx_Temperature = 0;//接收的温度,℃

    uint16_t Encoder_Num_Per_Round = 8192;//一圈编码器刻度
    uint16_t Pre_Encoder = 0;//先前编码器位置
    int32_t Total_Encoder = 0;//总编码器位置
    int32_t Total_Round = 0;//总圈数
    uint32_t Test_cnt = 0;//测试模式计时器

    CAN_Object can;
};

#endif //M2006_RE_DRV_M2006_H

